#include "common_inc.h"
#include "fsl_csi.h"

DRV8701E_RT1064 lf_driver(PWM1, kPWM_Module_3, kPWM_Control_Module_3, kPWM_PwmA, GPIO2, 5U); // m1
DRV8701E_RT1064 rb_driver(PWM1, kPWM_Module_3, kPWM_Control_Module_3, kPWM_PwmB, GPIO2, 6U); // rb
DRV8701E_RT1064 rf_driver(PWM2, kPWM_Module_3, kPWM_Control_Module_3, kPWM_PwmB, GPIO8, 4U); // motor4
DRV8701E_RT1064 lb_driver(PWM2, kPWM_Module_3, kPWM_Control_Module_3, kPWM_PwmA, GPIO2, 7U); // lb

Encoder enc_lf(TMR1, kQTMR_Channel_0, kQTMR_PriSrcRiseEdgeSecDir, kQTMR_ClockCounter0InputPin, kQTMR_Counter1InputPin, 0.01f); // 正转增加
Encoder enc_lb(TMR3, kQTMR_Channel_2, kQTMR_PriSrcRiseEdgeSecDir, kQTMR_ClockCounter2InputPin, kQTMR_Counter3InputPin, 0.01f); // 正转增加
Encoder enc_rf(TMR1, kQTMR_Channel_2, kQTMR_PriSrcRiseEdgeSecDir, kQTMR_ClockCounter2InputPin, kQTMR_Counter3InputPin, 0.01f); // 往后增加
Encoder enc_rb(TMR2, kQTMR_Channel_0, kQTMR_PriSrcRiseEdgeSecDir, kQTMR_ClockCounter0InputPin, kQTMR_Counter3InputPin, 0.01f); // 往后增加
ADC_RT1064 adc(4, BOARD_ADC1_channels_config, BOARD_ADC1_CH0_CONTROL_GROUP, ADC1);
Motor MLF(&lf_driver, &adc, &enc_lf);	 // id=1
Motor MLB(&lb_driver, &adc, &enc_lb);	 // id=4
Motor MRF(&rf_driver, &adc, &enc_rf);	 // id=2
Motor MRB(&rb_driver, &adc, &enc_rb);	 // id=3   10%占空比，空载930

Usart_RT1064 usart1(LPUART1, &BOARD_LPUART1_handle, BOARD_LPUART1_txBuffer, BOARD_LPUART1_rxBuffer, BOARD_LPUART1_TX_BUFFER_SIZE, BOARD_LPUART1_RX_BUFFER_SIZE, &BOARD_LPUART1_txTransfer, &BOARD_LPUART1_rxTransfer);
Usart_RT1064 usart3(LPUART3, &BOARD_LPUART3_handle, BOARD_LPUART3_txBuffer, BOARD_LPUART3_rxBuffer, BOARD_LPUART3_TX_BUFFER_SIZE, BOARD_LPUART3_RX_BUFFER_SIZE, &BOARD_LPUART3_txTransfer, &BOARD_LPUART3_rxTransfer);
Icm42688 icm;
QMC5883P_RT1064 qmc(LPI2C1, Normal);
TFT_RT1064 tft(LPSPI3,2,160,128);
Chassis chassis(&MLF, &MRF, &MLB, &MRB,&icm); // 右边reverse是向前，左边是向后
float speed[8];
uint8_t mt9v03x_finish_flag = false;

float x_target = 0.0f;
float y_target = 0.0f;
float yaw_target = 0.0f;
uint8_t camera_send_image_frame_header[4] = {0x00, 0xFF, 0x01, 0x01};

volatile uint8_t enc_sample_flag = false;
IIC_t iic=
{
	.scl_base = GPIO2,
	.scl_pin = 29U,
	.sda_base = GPIO2,
	.sda_pin = 28U
};
MT9V034_RT1064 camera(&iic,CSI,&BOARD_CSI_handle);

void BOARD_PIT_IRQHANDLER()
{
	PIT_ClearStatusFlags(BOARD_PIT_PERIPHERAL, BOARD_PIT_CHANNEL_0, kPIT_TimerFlag);
//	speed[0] = MLF.GetSpeed();
//	speed[1] = MRF.GetSpeed();
//	speed[2] = MLB.GetSpeed();
//	speed[3] = MRB.GetSpeed();
	speed[0] = chassis.yawAngleLoop.Target;
	speed[1] = chassis.yawAngleLoop.Current;
	speed[2] = chassis.yawSpeedLoop.Target;
	speed[3] = chassis.yawSpeedLoop.Current;
	speed[4] = chassis.yawAngleLoop.Out;
	speed[5] = chassis.yawSpeedLoop.Out;
	usart1.SendData(speed, 8);
	if (!enc_sample_flag)
	{
		enc_sample_flag = 1;
	}
	SDK_ISR_EXIT_BARRIER;
}

void CSI_IRQHandler(void)
{
    CSI_DriverIRQHandler();     // 调用SDK自带的中断函数 这个函数最后会调用我们设置的回调函数
    __DSB();                    // 数据同步隔离
}

void mt9v03x_finished_callback(CSI_Type *base, csi_handle_t *handle, status_t status, void *userData)
{
		uint32_t full_buffer_addr;
    if(kStatus_Success == CSI_TransferGetFullBuffer(base,handle,(uint32_t *)&full_buffer_addr))
    {
			CSI_TransferSubmitEmptyBuffer(base,handle,full_buffer_addr);
			if(full_buffer_addr == (uint32_t)BOARD_CSI_Buffer[0])
			{
				camera.mt9v03x_image = BOARD_CSI_Buffer[0];
			}else if (full_buffer_addr == (uint32_t)BOARD_CSI_Buffer[1])
			{
				camera.mt9v03x_image = BOARD_CSI_Buffer[1];
			}
			mt9v03x_finish_flag = 1;
    }
}

void UserMain()
{
	//	lf_driver.DRV_Init();
	//	rf_driver.DRV_Init();
	//	lb_driver.DRV_Init();
	//	rb_driver.DRV_Init();
	////	adc.Init();
	//	lf_driver.Reverse(15535);
	//	rf_driver.Reverse(15535);
	//	lb_driver.Reverse(15535);
	//	rb_driver.Reverse(15535);
	// enc_lf.Init();
	// enc_lb.Init();
	// enc_rf.Init();
	// enc_rb.Init();
	// MLF.Init();
	// MRF.Init();
	// MLB.Init();
	// MRB.Init();
	// lf_driver.Forward(6553);
	/*int rc;
	icm.ConfigInit(inv_io_hal_read_reg, inv_io_hal_write_reg, 1024 * 32, 1024 * 32);
	rc = icm.SetupInvDevice(&icm.icm_serif, true);

	rc = icm.InitInvAGMBiases();
	rc |= icm.InitInvAGMAlgo();
	rc = icm.ConfigureInvDevice((uint8_t)IS_LOW_NOISE_MODE, 2,
								2000, ICM426XX_ACCEL_CONFIG0_ODR_1_KHZ,
								ICM426XX_GYRO_CONFIG0_ODR_1_KHZ, (uint8_t)USE_CLK_IN);
*/
	// qmc.SelfTest();

	// qmc.SetUp();
	//IIC_Init(&iic);
	chassis.Init();
	camera.Init();
	tft.LCD_Init();
	//lf_driver.Reverse(5535);
		//rf_driver.Reverse(5535);
		//lb_driver.Reverse(5535);
	//	rb_driver.Reverse(5535);

	while (1)
	{
		if (enc_sample_flag)
		{
			chassis.Update(&x_target, &y_target, &yaw_target);
			chassis.Adjust();
			chassis.Acuate();
			//tft.LCD_ShowPicture(0,0,160,128,(const uint8_t *)camera.mt9v03x_image);
			enc_sample_flag = false;
			//SCCB_Device_Read16bit(&iic,0x5C,0x00);
//			SCCB_Device_Read16bit(&iic,0x5C,0x03);
//			SCCB_Device_Read16bit(&iic,0x5C,0x04);
//			SCCB_Device_Read16bit(&iic,0x5C,0x03);
//			SCCB_Device_Read16bit(&iic,0x5C,0x0B);
			//vision_main(camera.mt9v03x_image);
			
		}
		tft.tft180_show_gray_image(0,0,(const uint8_t *)camera.mt9v03x_image,188,120,160,128,0);

// if(mt9v03x_finish_flag){
//			LPUART_WriteBlocking(LPUART1, camera_send_image_frame_header, 4);
//			LPUART_WriteBlocking(LPUART1, (const uint8_t *)camera.mt9v03x_image, 188*120);
// }
 
	//vision_main(camera.mt9v03x_image);
		

//		if (irq_from_device & TO_MASK(1))
//		{
//			// rc = icm.GetDataFromInvDevice();
//			rc = icm.GetDataFromInvDeviceFIFO();
//			icm.fixedpoint_to_float(Icm42688::output.grv_quat_q30, rvquat, 30, 4);
//			icm.quaternions_to_angles(rvquat);
//			inv_disable_irq();
//			irq_from_device &= ~TO_MASK(1);
//			inv_enable_irq();
//		}

		/*MLF.Update();
	MRF.Update();
	MLB.Update();
	MRB.Update();*/
		// usart3.SendCommonData(data, 4);

		// usart1.SendCommonData();
	}
}
